#include "servo.h"
#include <stdio.h>
#include <math.h>
#include <stdlib.h>


Servo_t *s1;
Servo_t *s2;

const float x0_pixels = 72.0f;  
const float y0_pixels = 20.0f;

Servo_t* servo_InitStruct(TIM_HandleTypeDef *htim, uint32_t channel, int p, float range, uint16_t duty_max, uint16_t duty_min)
{
	Servo_t *s = (Servo_t *)malloc(sizeof(Servo_t));
  if(s == NULL) return NULL;
	
	s->duty_max = duty_max;
	s->duty_min = duty_min;
	s->htim = htim;
	s->channel = channel;
	s->polarity = p;
	s->range = range; 
	HAL_TIM_PWM_Start(s->htim, s->channel);
	return s;
}

void servo_SetDutyAbs(Servo_t* s, uint16_t duty)
{
	s->duty_abs = duty;
	if(s->duty_abs > s->duty_max) s->duty_abs = s->duty_max;
	else if(s->duty_abs < s->duty_min) s->duty_abs = s->duty_min;
	__HAL_TIM_SET_COMPARE(s->htim, s->channel, s->duty_abs);
}

void servo_SetDutyRela(Servo_t* s, int duty)
{
	s->duty_abs += s->polarity * duty;
	if(s->duty_abs > s->duty_max) s->duty_abs = s->duty_max;
	else if(s->duty_abs < s->duty_min) s->duty_abs = s->duty_min;
	__HAL_TIM_SET_COMPARE(s->htim, s->channel, s->duty_abs);
}

void servo_SetAngleAbs(Servo_t* s, float angle)
{
	int duty = (uint16_t)(angle / (s->range) * 2000 + 500);
	servo_SetDutyAbs(s, duty);
}

void servo_SetAngleRela(Servo_t* s, float angle)
{
	int duty = (int)(angle / (s->range) * 2000);
	servo_SetDutyRela(s, duty);
}

/* 求解起点到终点，XY轴舵机旋转角度 */
void servo_CalculateAngle(int pixel_x1, int pixel_y1,int pixel_x2, int pixel_y2,float* x_angle, float* y_angle) 
{
    // 简单像素到米转换
		float x1 = (pixel_x1 - x0_pixels) * PIEXL2M_X;
    float y1 = (y0_pixels - pixel_y1) * PIEXL2M_Y;  // Y轴翻转
    float x2 = (pixel_x2 - x0_pixels) * PIEXL2M_X;
    float y2 = (y0_pixels - pixel_y2) * PIEXL2M_Y;
    
    // 计算X轴(水平)舵机角度
    float x_angle1 = atan2f(x1, WALL_DISTANCE) * 180.0f / M_PI;
    float x_angle2 = atan2f(x2, WALL_DISTANCE) * 180.0f / M_PI;
    *x_angle = x_angle2 - x_angle1;
    
    // 计算Y轴(垂直)舵机角度
    float y_angle1 = atan2f(y1, WALL_DISTANCE) * 180.0f / M_PI;
    float y_angle2 = atan2f(y2, WALL_DISTANCE) * 180.0f / M_PI;
    *y_angle = y_angle2 - y_angle1;
}

///* 求解起点到终点，XY轴舵机旋转角度 */
//void servo_CalculateAngle(int pixel_x1, int pixel_y1,int pixel_x2, int pixel_y2,float* x_angle, float* y_angle) 
//{
//    // 简单像素到米转换
//		float x1 = (pixel_x1 - x0_pixels) * PIEXL2M;
//    float y1 = (y0_pixels - pixel_y1) * PIEXL2M;  // Y轴翻转
//    float x2 = (pixel_x2 - x0_pixels) * PIEXL2M;
//    float y2 = (y0_pixels - pixel_y2) * PIEXL2M;
//    
//    // 计算X轴(水平)舵机角度
//    float x_angle1 = atan2f(x1, WALL_DISTANCE) * 180.0f / M_PI;
//    float x_angle2 = atan2f(x2, WALL_DISTANCE) * 180.0f / M_PI;
//    *x_angle = x_angle2 - x_angle1;
//    
//    // 计算Y轴(垂直)舵机角度
//    float hyp1 = sqrtf(x1*x1 + WALL_DISTANCE*WALL_DISTANCE);
//    float y_angle1 = atan2f(y1, hyp1) * 180.0f / M_PI;
//    
//    float hyp2 = sqrtf(x2*x2 + WALL_DISTANCE*WALL_DISTANCE);
//    float y_angle2 = atan2f(y2, hyp2) * 180.0f / M_PI;
//    *y_angle = y_angle2 - y_angle1;
//}

/*********** 细分 ************/
uint16_t i = 0;
uint16_t st = 1;
#define PID_RANGE 15

void servo_Pursue(void)
{
	pid1.Actual = Laser_x;
	pid2.Actual = Laser_y;
	PID_Positional_Calculate(&pid1);
	PID_Positional_Calculate(&pid2);
	servo_SetDutyRela(s1, pid1.Out);
	servo_SetDutyRela(s2, -pid2.Out);
}

void servo_process1(PID_Positional_t *pid1, PID_Positional_t *pid2, uint16_t section)
{
	switch(st)
	{
		case 1:
			servo_Pursue();
			if(fabs(pid1->Target - Laser_x) < PID_RANGE && fabs(pid2->Target - Laser_y) < PID_RANGE) 
			{
				pid1->Target = (X_1 * (section - i) + X_2 * i) / section; // 目标x位置
        pid2->Target = (Y_1 * (section - i) + Y_2 * i) / section; // 目标y位置
				i++;
				if(i > section)
				{
					i = 0;
					pid_clear(pid1), pid_clear(pid2);
					st = 2;
				}
			}
			break;
		
		case 2:
			servo_Pursue();
			if(fabs(pid1->Target - Laser_x) < PID_RANGE && fabs(pid2->Target - Laser_y) < PID_RANGE) 
			{
				pid1->Target = (X_2 * (section - i) + X_3 * i) / section; // 目标x位置
        pid2->Target = (Y_2 * (section - i) + Y_3 * i) / section; // 目标y位置
				i++;
				if(i > section)
				{
					i = 0;
					pid_clear(pid1), pid_clear(pid2);
					st = 3;
				}
			}
			break;
		
		case 3:
			servo_Pursue();
			if(fabs(pid1->Target - Laser_x) < PID_RANGE && fabs(pid2->Target - Laser_y) < PID_RANGE) 
			{
				pid1->Target = (X_3 * (section - i) + X_4 * i) / section; // 目标x位置
        pid2->Target = (Y_3 * (section - i) + Y_4 * i) / section; // 目标y位置
				i++;
				if(i > section)
				{
					i = 0;
					pid_clear(pid1), pid_clear(pid2);
					st = 4;
				}
			}
			break;
		
		case 4:
			servo_Pursue();
			if(fabs(pid1->Target - Laser_x) < PID_RANGE && fabs(pid2->Target - Laser_y) < PID_RANGE) 
			{
				pid1->Target = (X_4 * (section - i) + X_1 * i) / section; // 目标x位置
        pid2->Target = (Y_4 * (section - i) + Y_1 * i) / section; // 目标y位置
				i++;
				if(i > section)
				{
					i = 0;
					pid_clear(pid1), pid_clear(pid2);
					st = 5;
				}
			}
			break;
		case 5:
			
			break;
		default:
			break;
	}
	
	printf("%d,%d,%d,%d,%d,%d\n",(int)pid1->Target,(int)pid2->Target,Laser_x,Laser_y, s1->duty_abs, s2->duty_abs);
}



